/*
 * Copyright (c) 2015-2016, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/***** Includes *****/
#include <stdlib.h>
#include <xdc/std.h>
#include <xdc/cfg/global.h>
#include <xdc/runtime/System.h>

#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Semaphore.h>
#include <xdc/runtime/Error.h>

/* Drivers */
#include <ti/drivers/rf/RF.h>
#include <ti/drivers/PIN.h>
#include <ti/drivers/pin/PINCC26XX.h>

#ifdef DEVICE_FAMILY
    #undef DEVICE_FAMILY_PATH
    #define DEVICE_FAMILY_PATH(x) <ti/devices/DEVICE_FAMILY/x>
    #include DEVICE_FAMILY_PATH(driverlib/rf_prop_mailbox.h)
#else
    #error "You must define DEVICE_FAMILY at the project level as one of cc26x0, cc26x0r2, cc13x0, etc."
#endif

/* Board Header files */
#include "Board.h"

#include "RFQueue.h"
#include "smartrf_settings/smartrf_settings.h"

#include "rfBleNcTx.h"

#include <stdlib.h>

#define ms_To_RadioTime(ms) (ms*(4000000/1000))
#define RadioTime_To_ms(radioTime) ((1000 * radioTime) / 4000000)

//1ms scheduling margin to take in to account power up/down and patch timing
#define schedule_margin (1*(4000000/1000))

/* Pin driver handle */
static PIN_Handle ledPinHandle;
static PIN_State ledPinState;

#define RX_ON_GPIO  Board_PIN_LED1
#define TX_ON_GPIO  Board_PIN_LED2
#define S1_RX_CMD   CC1350_LAUNCHXL_DIO15
#define S1_SUCCESS  CC1350_LAUNCHXL_DIO12
#define S1_ABORT    IOID_8


/*
 * Application LED pin configuration table:
 *   - All LEDs board LEDs are off.
 */
PIN_Config pinTable[] =
{
    TX_ON_GPIO | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    RX_ON_GPIO | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    S1_RX_CMD | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    S1_SUCCESS | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    S1_ABORT | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    BLE_TX | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    BLE_ABORT | PIN_GPIO_OUTPUT_EN | PIN_GPIO_LOW | PIN_PUSHPULL | PIN_DRVSTR_MAX,
    PIN_TERMINATE
};


/***** Defines *****/
#define RX_TASK_STACK_SIZE 4096
#define RX_TASK_PRIORITY   2

/* Packet RX Configuration */
#define DATA_ENTRY_HEADER_SIZE 8  /* Constant header size of a Generic Data Entry */
#define MAX_LENGTH             32 /* Max length byte the radio will accept */
#define NUM_DATA_ENTRIES       2  /* NOTE: Only two data entries supported at the moment */
#define NUM_APPENDED_BYTES     2  /* The Data Entries data field will contain:
                                   * 1 Header byte (RF_cmdPropRx.rxConf.bIncludeHdr = 0x1)
                                   * Max 30 payload bytes
                                   * 1 status byte (RF_cmdPropRx.rxConf.bAppendStatus = 0x1) */

#define RF_EVENT_MASK  ( RF_EventLastCmdDone | RF_EventCmdError | \
             RF_EventCmdAborted | RF_EventCmdStopped | RF_EventCmdCancelled )

/***** Prototypes *****/
static void rxTaskFunction(UArg arg0, UArg arg1);
void scheduleRxCmd(RF_Handle h,  rfc_CMD_PROP_RX_t *pRxCmd, RF_ScheduleCmdParams *pSchParams, RF_Callback pAppCb);
void scheduleRxCb(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
void scheduleSetRxCmdTiming(RF_Handle h, rfc_CMD_PROP_RX_t *pRxCmd, RF_ScheduleCmdParams *pSchParams);

/***** Variable declarations *****/
static Task_Params rxTaskParams;
Task_Struct rxTask;    /* not static so you can see in ROV */
static uint8_t rxTaskStack[RX_TASK_STACK_SIZE];

static RF_Object rfObject;
static RF_Handle rfHandle;

/* Buffer which contains all Data Entries for receiving data.
 * Pragmas are needed to make sure this buffer is 4 byte aligned (requirement from the RF Core) */
#if defined(__TI_COMPILER_VERSION__)
    #pragma DATA_ALIGN (rxDataEntryBuffer, 4);
        static uint8_t rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES,
                                                                 MAX_LENGTH,
                                                                 NUM_APPENDED_BYTES)];
#elif defined(__IAR_SYSTEMS_ICC__)
    #pragma data_alignment = 4
        static uint8_t rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES,
                                                                 MAX_LENGTH,
                                                                 NUM_APPENDED_BYTES)];
#elif defined(__GNUC__)
        static uint8_t rxDataEntryBuffer [RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES,
            MAX_LENGTH, NUM_APPENDED_BYTES)] __attribute__ ((aligned (4)));
#else
    #error This compiler is not supported.
#endif

/* Receive dataQueue for RF Core to fill in data */
static dataQueue_t dataQueue;
static rfc_dataEntryGeneral_t* currentDataEntry;
static uint8_t packetLength;
static uint8_t* packetDataPointer;

static PIN_Handle pinHandle;

static uint8_t packet[MAX_LENGTH + NUM_APPENDED_BYTES - 1]; /* The length byte is stored in a separate variable */

static bool schedulerTruncated = false;
//Mutex for locking the RF driver resource
static Semaphore_Handle schRxCompleteSem;
static uint16_t rxOkCnt = 0;
static rfc_propRxOutput_t rxStatistics = {0};
static uint32_t rxEndTime;
static RF_ScheduleCmdParams schParams;

/***** Function definitions *****/
void RxTask_init(PIN_Handle ledPinHandle) {
    pinHandle = ledPinHandle;

    Task_Params_init(&rxTaskParams);
    rxTaskParams.stackSize = RX_TASK_STACK_SIZE;
    rxTaskParams.priority = RX_TASK_PRIORITY;
    rxTaskParams.stack = &rxTaskStack;
    rxTaskParams.arg0 = (UInt)1000000;

    Task_construct(&rxTask, rxTaskFunction, &rxTaskParams, NULL);
}

static void rxTaskFunction(UArg arg0, UArg arg1)
{
    RF_Params rfParams;
    RF_Params_init(&rfParams);
    //Create a semaphore for blocking commands
    Semaphore_Params params;
    Error_Block eb;

    // init params
    Semaphore_Params_init(&params);
    Error_init(&eb);

    schRxCompleteSem = Semaphore_create(0, &params, &eb);


    if( RFQueue_defineQueue(&dataQueue,
                            rxDataEntryBuffer,
                            sizeof(rxDataEntryBuffer),
                            NUM_DATA_ENTRIES,
                            MAX_LENGTH + NUM_APPENDED_BYTES))
    {
        /* Failed to allocate space for all data entries */
        while(1);
    }

    /* Modify CMD_PROP_RX command for application needs */
    RF_cmdPropRx.pQueue = &dataQueue;           /* Set the Data Entity queue for received data */
    RF_cmdPropRx.rxConf.bAutoFlushIgnored = 1;  /* Discard ignored packets from Rx queue */
    RF_cmdPropRx.rxConf.bAutoFlushCrcErr = 1;   /* Discard packets with CRC error from Rx queue */
    RF_cmdPropRx.maxPktLen = MAX_LENGTH;        /* Implement packet length filtering to avoid PROP_ERROR_RXBUF */
    RF_cmdPropRx.pktConf.bRepeatOk = 0;
    RF_cmdPropRx.pktConf.bRepeatNok = 0;

    RF_prop.rfMode = RF_MODE_MULTIPLE;


    /* Request access to the radio */
    rfHandle = RF_open(&rfObject, &RF_prop, (RF_RadioSetup*)&RF_cmdPropRadioDivSetup, &rfParams);

    /* Set the frequency */
    schParams.priority = RF_PriorityHigh;
    RF_scheduleCmd(rfHandle, (RF_Op*)&RF_cmdFs, &schParams, NULL, 0);

    while(1)
    {
        uint32_t currentTime;

        RF_cmdPropRx.pOutput = (uint8_t*) &rxStatistics;
        rxStatistics.nRxOk = 0;

        RF_cmdPropRx.startTrigger.triggerType = TRIG_ABSTIME;
        //Set past trigger in case the other client preempts the command
        RF_cmdPropRx.startTrigger.pastTrig = 1;
        RF_cmdPropRx.startTime = RF_getCurrentTime() + ms_To_RadioTime(150);
        RF_cmdPropRx.endTrigger.triggerType = TRIG_ABSTIME;
        //This is important, without setting endTime.pastTrigger the command may not end!
        RF_cmdPropRx.endTrigger.pastTrig = 1;
        RF_cmdPropRx.endTime = RF_cmdPropRx.startTime + ms_To_RadioTime(150);
        schParams.priority = RF_PriorityNormal;
        schParams.endTime = RF_cmdPropRx.endTime;

        PIN_setOutputValue(ledPinHandle, S1_RX_CMD, 1);
        scheduleRxCmd(rfHandle, &RF_cmdPropRx, &schParams, 0);
        PIN_setOutputValue(pinHandle, S1_RX_CMD, 0);

        //Did we have an RX and need to re-align timing?
        currentTime = RF_getCurrentTime();
        if (currentTime < RF_cmdPropRx.endTime)
        {
            /*
             * RX exited due to error a successful Rx
             */

            //1. Deal with the Rx packet

            //2. Sleep until the next RX cycle
            uint32_t sleepTime;

            //get sleep time in us
            sleepTime = RadioTime_To_ms((RF_cmdPropRx.endTime - currentTime)) * 1000;
            //get sleepTime in Ticks
            sleepTime = sleepTime / Clock_tickPeriod;

            Task_sleep(sleepTime);
        }
    }
}

void scheduleRxCb(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
    bool rxComplete = true;
    bool schedulerTruncatedTmp = schedulerTruncated;

    if ((e & RF_EventLastCmdDone) && (rxStatistics.nRxOk > 0))
    {
        PIN_setOutputValue(pinHandle, S1_SUCCESS, 1);
        rxOkCnt++;

        /* Get current unhandled data entry */
        currentDataEntry = RFQueue_getDataEntry();

        /* Handle the packet data, located at &currentDataEntry->data:
         * - Length is the first byte with the current configuration
         * - Data starts from the second byte */
        packetLength      = *(uint8_t*)(&currentDataEntry->data);
        packetDataPointer = (uint8_t*)(&currentDataEntry->data + 1);

        /* Copy the payload + the status byte to the packet variable */
        memcpy(packet, packetDataPointer, (packetLength + 1));

        RFQueue_nextEntry();

        schedulerTruncatedTmp = schedulerTruncated = false;
    }

    else if (e & ( RF_EventCmdAborted | RF_EventCmdStopped | RF_EventCmdCancelled))
    {
        PIN_setOutputValue(pinHandle, S1_ABORT, 1);

        /*
         * command was aborted due to higher priority client scheduling a command,
         * resubmit the command with start time of now + a margin
         */
        RF_cmdPropRx.startTime = RF_getCurrentTime() + (10*schedule_margin);
        schedulerTruncated = false;

        scheduleSetRxCmdTiming(rfHandle, &RF_cmdPropRx, &schParams);
        RF_scheduleCmd(h, (RF_Op*)&RF_cmdPropRx, &schParams, scheduleRxCb, RF_EVENT_MASK);

        rxComplete = false;
    }

    if (schedulerTruncatedTmp == true)
    {
        RF_cmdPropRx.startTime = RF_cmdPropRx.endTime + (2*schedule_margin);
        schedulerTruncated = false;
        RF_cmdPropRx.endTime = rxEndTime;
        schParams.endTime = rxEndTime;

        scheduleSetRxCmdTiming(rfHandle, &RF_cmdPropRx, &schParams);
        RF_scheduleCmd(h, (RF_Op*)&RF_cmdPropRx, &schParams, scheduleRxCb, RF_EVENT_MASK);

        rxComplete = false;
    }


    if(rxComplete)
    {
        Semaphore_post(schRxCompleteSem);
    }

    PIN_setOutputValue(ledPinHandle, S1_SUCCESS, 0);
    PIN_setOutputValue(ledPinHandle, S1_ABORT, 0);
}

void scheduleSetRxCmdTiming(RF_Handle h, rfc_CMD_PROP_RX_t *pRxCmd, RF_ScheduleCmdParams *pSchParams)
{
    RF_ScheduleMap RF_scheduleMap;
    RF_InfoVal RF_getInfoVal;
    uint8_t schMapCmdIdx = 0;

    // Get schedule map
    RF_getInfoVal.pScheduleMap = &RF_scheduleMap;
    RF_getInfo(NULL, RF_GET_SCHEDULE_MAP, &RF_getInfoVal);

    for(schMapCmdIdx = 0; schMapCmdIdx < 3; schMapCmdIdx++)
    {
        //check for commands from the other client
        if ( (RF_scheduleMap.commandMap[schMapCmdIdx].pClient != 0) &&
             (RF_scheduleMap.commandMap[schMapCmdIdx].pClient != h) )
        {
            //if other client command start overlaps our command window
            if ( (pRxCmd->startTime <= RF_scheduleMap.commandMap[schMapCmdIdx].startTime) &&
                 (pSchParams->endTime >= (RF_scheduleMap.commandMap[schMapCmdIdx].startTime - (schedule_margin)))   )

            {
                schedulerTruncated = true;
                rxEndTime = pRxCmd->endTime;

                //Modify endTime to end before other command starts
                pSchParams->endTime = RF_scheduleMap.commandMap[schMapCmdIdx].startTime - schedule_margin;
                pRxCmd->endTime = pSchParams->endTime;
            }

            //if other client command end overlaps our command window
            if ( (pRxCmd->startTime >= (RF_scheduleMap.commandMap[schMapCmdIdx].startTime - (schedule_margin))) &&
                 (pRxCmd->startTime <= RF_scheduleMap.commandMap[schMapCmdIdx].endTime)   )
            {
                //Modify StartTime to start after other command ends
                pRxCmd->startTime = RF_scheduleMap.commandMap[schMapCmdIdx].endTime + schedule_margin;
            }
        }
    }
}

void scheduleRxCmd(RF_Handle h,  rfc_CMD_PROP_RX_t *pRxCmd, RF_ScheduleCmdParams *pSchParams, RF_Callback pAppCb)
{
    RF_CmdHandle cmdHdl;

    scheduleSetRxCmdTiming(h, pRxCmd, pSchParams);

    cmdHdl = RF_scheduleCmd(h, (RF_Op*)pRxCmd, pSchParams, scheduleRxCb, RF_EVENT_MASK);

    if(cmdHdl == -2)
    {
        PIN_setOutputValue(pinHandle, S1_ABORT, 1);
    }
    else
    {
        Semaphore_pend(schRxCompleteSem, BIOS_WAIT_FOREVER);
    }
}

/*
 *  ======== main ========
 */
int main(void)
{
    /* Call driver init functions. */
    Board_initGeneral();

    /* Open LED pins */
    ledPinHandle = PIN_open(&ledPinState, pinTable);
    if(!ledPinHandle)
    {
        System_abort("Error initializing board LED pins\n");
    }

    /* Set Tx/Rx Pins */
    //LNA Enable - Rx
    PINCC26XX_setMux(ledPinHandle, RX_ON_GPIO, PINCC26XX_MUX_RFC_GPO0);
    //PA Enable - Tx
    PINCC26XX_setMux(ledPinHandle, TX_ON_GPIO, PINCC26XX_MUX_RFC_GPO1);

    /* Clear Debug pins */
    PIN_setOutputValue(ledPinHandle, S1_ABORT, 0);
    PIN_setOutputValue(ledPinHandle, S1_SUCCESS, 0);
    PIN_setOutputValue(ledPinHandle, BLE_ABORT, 0);

    /* Initialize RF task */
    RxTask_init(ledPinHandle);

    /* Initialize RF task */
    bleNcTask_init(ledPinHandle);

    /* Start BIOS */
    BIOS_start();

    return (0);
}
